#include "main.h"
#include "control.h"
#include "can_receive.h"
#include "remote_control.h"
#include <math.h>
#include "mecanum.h"


extern PidTypeDef PID_Motor[4];

#define LimitMax(input, max)\
{\
    if (input > max)\
  {\
        input = max;\
  }\
  else if (input < -max)\
  {\
        input = -max;\
  }\
}




void PID_init(PidTypeDef *pid,uint8_t mode,const fp32 PID[3],fp32 max_out,fp32 max_iout)
{
    if(pid==NULL||PID==NULL)
    {
        return;
    }
    pid->mode=mode;
    pid->Kp=PID[0];
    pid->Ki=PID[1];
    pid->Kd=PID[2];
    pid->max_out=max_out;
    pid->max_iout=max_iout;
    pid->Dbuf[0]=pid->Dbuf[1]=pid->Dbuf[2]=0.0f;
    pid->error[0]=pid->error[1]=pid->error[2]=pid->Pout=pid->Iout=pid->Dout=pid->out=0.0f;
}
 
fp32 PID_Calc(PidTypeDef *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }
 
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = set - ref;
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        LimitMax(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    return pid->out;
}


void Motor_Control(void)
{
    int16_t motor_current[4] = {0};
    fp32 current_speed[4] = {0};
    
    
    for(int i = 0;i<4;i++)
    {
       current_speed[i]=get_chassis_motor_measure_point(i)->speed_rpm;
    }

    motor_current[0] = (int16_t)(PID_Calc(&PID_Motor[0],current_speed[0],get_wheel_speed()->wheel_0));
    motor_current[1] = (int16_t)(PID_Calc(&PID_Motor[1],current_speed[1],get_wheel_speed()->wheel_1));
    motor_current[2] = (int16_t)(PID_Calc(&PID_Motor[2],current_speed[2],get_wheel_speed()->wheel_2));
    motor_current[3] = (int16_t)(PID_Calc(&PID_Motor[3],current_speed[3],get_wheel_speed()->wheel_3));

    if(get_remote_control_point()->rc.s[1] == 1)
    {
      CAN_cmd_chassis(0,0,0,0);
    }
    else
    {
      CAN_cmd_chassis(motor_current[0],motor_current[1],motor_current[2],motor_current[3]);
      
    }
    
}


